#ifndef __HELPER_UTILS_H__
#define __HELPER_UTILS_H__

#include <ros/ros.h>
#include <tf2/utils.h>

#include <eigen3/Eigen/Dense>
using namespace std;

namespace helper{

    /**
     * @description: 四元数转偏航角
     * @param {Quaternion&} q
     * @return {*}
     */    
    double quaternionToYaw(const geometry_msgs::Quaternion& q){
        return tf2::getYaw(q);
    }


    /**
     * @description: 四元数转欧拉角 roll, pitch, yaw
     * @param {Quaternion&} q
     * @return {*}
     */    
    Eigen::Vector3d quaternionToEuler(const geometry_msgs::Quaternion& q){
        double roll, pitch, yaw;
        // // 方式一
        // tf2::Quaternion q_tf2;  
        // tf2::convert(q, q_tf2);
        // tf2::Matrix3x3 m(q_tf2);
        // m.getRPY(roll, pitch, yaw);
        // cout << "[getRPY] yaw: " << yaw << ", pitch: " << pitch << ", roll: " << roll << endl;

        // 方式二
        tf2::getEulerYPR(q, yaw, pitch, roll);
        // cout << "[getEulerYPR] yaw: " << yaw << ", pitch: " << pitch << ", roll: " << roll << endl;

        // cout << "[getYaw] yaw: " << tf2::getYaw(q) << endl;
        return Eigen::Vector3d(roll, pitch, yaw);
    }


    /**
     * @description: 时间转时间戳 2022-10-08 10:33:56 -> 1665196436
     * @param {string} time
     * @return {*}
     */
    long standard_to_stamp(string time){
        const char*str_time = time.c_str();
        struct tm stm;
        int iY, iM, iD, iH, iMin, iS;
        memset(&stm, 0, sizeof(stm));
        iY = atoi(str_time);
        iM = atoi(str_time + 5);
        iD = atoi(str_time + 8);
        iH = atoi(str_time + 11);
        iMin = atoi(str_time + 14);
        iS = atoi(str_time + 17);

        stm.tm_year = iY - 1900;
        stm.tm_mon = iM - 1;
        stm.tm_mday = iD;
        stm.tm_hour = iH;
        stm.tm_min = iMin;
        stm.tm_sec = iS;
        return (long)mktime(&stm);
    }


    /**
     * @description: 时间戳转时间 1665196436 -> 2022-10-08 10:33:56 
     * @param {long} stampTime
     * @return {*}
     */
    std::string stamp_to_standard(long stampTime){
        time_t tick = (time_t)stampTime;
        struct tm tm;
        char s[100];
        tm = *localtime(&tick);
        strftime(s, sizeof(s), "%Y-%m-%d %H:%M:%S", &tm);
        return  s;
    }


    // 角度转弧度
    float Deg2Rad(float angle){
        return  angle / 180.0 * M_PI;
    }

    // 弧度转角度
    float Rad2Deg(float angle){
        return  angle / M_PI * 180.0;
    }   

}



#endif



